import os
import cv2
import cv2.aruco as aruco
import numpy as np
import glob
import sys
import time
import ST7789
import threading
import spidev as SPI
from PIL import Image,ImageDraw,ImageFont
import RPi.GPIO as GPIO
from socket import *
from io import BytesIO
from picamera.array import PiRGBArray
from picamera import PiCamera

__author__ =  'Qing Bryce'
__version__ = '0.1 beta'
__author__ =  'Haoyang Zhang'
__version__ = '0.2 beta'

class Function0_Thread(threading.Thread):
	'''驱动1.54寸液显示屏'''
	def __init__(self):
		super(Function0_Thread, self).__init__()
		RST = 27
		DC = 25
		BL = 24
		bus = 0 
		device = 0 
		# 240x240 display with hardware SPI:
		self.disp = ST7789.ST7789(SPI.SpiDev(bus, device),RST, DC, BL)
		# Initialize library.
		self.disp.Init()
		# Clear display.
		self.disp.clear()

	def run(self):
		# Create blank image for drawing.
		image1 = Image.new("RGB", (self.disp.width, self.disp.height), "WHITE")
		draw = ImageDraw.Draw(image1)
		#font = ImageFont.truetype('/usr/share/fonts/truetype/freefont/FreeMonoBold.ttf', 16)
		#print "***draw line"
		draw.line([(60,60),(180,60)], fill = "BLUE",width = 5)
		draw.line([(180,60),(180,180)], fill = "BLUE",width = 5)
		draw.line([(180,180),(60,180)], fill = "BLUE",width = 5)
		draw.line([(60,180),(60,60)], fill = "BLUE",width = 5)
		#print "***draw rectangle"
		draw.rectangle([(70,70),(170,80)],fill = "RED")

		#print "***draw text"
		draw.text((90, 70),  '  YALONG  ', fill = "BLUE")
		draw.text((90, 100), 'MicroScope ', fill = "BLUE")
		draw.text((85, 140), '1.54inch LCD ', fill = "BLUE")
		self.disp.ShowImage(image1,0,0)
		time.sleep(3)
		
		while True:
			'''TFT显示功能'''
			global TFT_FLAG # 引入全局变量
			event.wait()
			if TFT_FLAG is True:
				MARK_IMG = Image.open("/home/pi/Desktop/MicroScope/Temp/MARK_IMG.jpg")
				self.disp.ShowImage(MARK_IMG,0,0)
			else:
				MicroScope_IMG = Image.open("/home/pi/Desktop/MicroScope/Temp/MicroScope_IMG.jpg")
				self.disp.ShowImage(MicroScope_IMG,0,0)
			time.sleep(0.01)
			event.clear()


class Function1_Thread(threading.Thread):
	'''驱动两个旋转编码器'''
	def __init__(self):
		super(Function1_Thread, self).__init__()
		# 旋转编码器初始化
		GPIO.setmode(GPIO.BCM)
		self.SIA = 5
		self.SIB= 6
		self.SW = 13
		self._SIA = 16
		self._SIB = 20
		self._SW = 21
		
		GPIO.setup(self.SW,GPIO.IN,pull_up_down=GPIO.PUD_UP)
		GPIO.setup(self.SIA,GPIO.IN,pull_up_down=GPIO.PUD_UP)
		GPIO.setup(self.SIB,GPIO.IN,pull_up_down=GPIO.PUD_UP)
		GPIO.setup(self._SW,GPIO.IN,pull_up_down=GPIO.PUD_UP)
		GPIO.setup(self._SIA,GPIO.IN,pull_up_down=GPIO.PUD_UP)
		GPIO.setup(self._SIB,GPIO.IN,pull_up_down=GPIO.PUD_UP)
			  
	def run(self):
		global EC11_FLAG0 # 全局变量 旋转编码器方向标志位 0：初始状态 1：正转 2：反转
		global EC11_FLAG1 # 全局变量 旋转编码器方向标志位 0：初始状态 1：正转 2：反转
		global Confirm # 确定mark中心位置为当前位置
		global TFT_FLAG
		global lastCenter  # Center position of mark 记录找到mark中心位置
		global loadedImg # Wheather the image has been loaded 图片是否已经被加载
		global Count_A
		global Count_B
		global comparekey
		global Speak

		flag = 0
		resetflag = 0
		Count_A = 0
		
		flag_B = 0
		resetflag_B = 0
		Count_B = 0
		
		while True:
			# 编码器A检测功能 x1
			lastSib = GPIO.input(self.SIB)
			while not GPIO.input(self.SW):
				resetflag = 1
			while not GPIO.input(self.SIA):
				currentSib = GPIO.input(self.SIB)
				flag =1
			if resetflag:
				comparekey = True
				# Count_A = 0
				resetflag = 0
				# print('A Count reset')
				continue
			if flag:
				if lastSib == 0 and currentSib == 1: # 正转
					# print('A right rotate')
					lock.acquire()
					EC11_FLAG0 = 1
					lock.release()
					Count_A += 1
				if lastSib == 1 and currentSib ==0: # 反转
					# print('A left rotate')
					lock.acquire()
					EC11_FLAG0 = 2
					lock.release()
					Count_A -=1
				flag =0
				# print('A current = %s' % Count_A)
				
			# 编码器B检测功能 x4
			lastSib_B = GPIO.input(self._SIB)
			while not GPIO.input(self._SW):
				resetflag_B = 1
			while not GPIO.input(self._SIA):
				currentSib_B = GPIO.input(self._SIB)
				flag_B = 1
			if resetflag_B:
				# print('jiu')
				# Count_B = 0
				Speak = not Speak
				if Speak:
					os.system('mplayer ./Sounds/0-打开语音提.mp3')
				else:
					os.system('mplayer ./Sounds/0-关闭语音提.mp3')

				resetflag_B = 0
				# lock.acquire()
				# TFT_FLAG = not TFT_FLAG
				# if TFT_FLAG == True:
				# 	Confirm = False
				# 	lastCenter = None
				# 	loadedImg = False
				# lock.release()
				# print('A Count reset %s %s' % (TFT_FLAG, Confirm))
				continue
			if flag_B:
				if lastSib_B == 0 and currentSib_B == 1: # 正转
					# print('A right rotate')
					lock.acquire()
					EC11_FLAG1 = 2
					lock.release()
					Count_B -= 1
				if lastSib_B == 1 and currentSib_B == 0: # 反转
					# print('A left rotate')
					lock.acquire()
					EC11_FLAG1 = 1
					lock.release()
					Count_B +=1
				flag_B =0
				# print('B current = %s' % Count_B)

class Stop_Thread(threading.Thread):
	def __init__(self):
		super(Stop_Thread, self).__init__()

	def run(self):
		global isFind
		global Timeout
		start = 0
		end = 0
		while True:
			time.sleep(0.1)
			if not First_key and not isFind:
				end = time.clock()
			else:
				start = time.clock()

			if end-start>=10:
				Timeout = True
			else:
				Timeout = False
			# print('ok', start, end)


class MicroScope:
	def __init__(self, micro_w, micro_h, micro_posX, micro_posY, dic_specimen_id ):
		"""[Init the microscope 初始化显微镜对象]
		Arguments:
			micro_w {[Number]} -- [The width of microscope 显微镜视口宽度]
			micro_h {[Number]} -- [The height of microsocpe 显微镜视口高度]
			micro_posX {[Number]} -- [The x postion of microscope 显微镜视口x坐标]
			micro_posY {[Number]} -- [The y position of microscope 显微镜视口y坐标]
			dic_specimen_id {[dict]} -- [The key-value : id: specimen 键值对: 图片id和标本路径]
		""" 
		# Detect the id and show the correspongding specimen photo 检测mark id并显示对应标本图片
		self.dic_specimen_id = dic_specimen_id	
		# The size of viewport of microscope. 显微镜视口大小
		self.microscope_w, self.microscope_h = micro_w, micro_h
		# Setting the name of window 设置窗口名称
		cv2.namedWindow('MicroScope', cv2.WINDOW_NORMAL)
		# Setting the window size 设置窗口大小
		cv2.resizeWindow('MicroScope', self.microscope_w, self.microscope_h)
		# Setting the window position 设置窗口位置
		cv2.moveWindow('MicroScope', micro_posX, micro_posY)
		# self.initViewPort()
		

	def FindMark(self, img, marktype = aruco.DICT_6X6_250):
		"""
		[Find mark corners]
		Arguments:
			img {[numpy.array]} -- [The input image 摄像头输入图像]
			marktype {[any]} -- [The dict of aurco mark Aruco mark的字典参数]
		Returns:
			(centerX, centerY) {[tuple]} --[The center position of mark 标记mark中心位置]
			markId {[Number]} --[id of mark 标记Mark的id]
			isFind {[Boolean]} --[Wheather find the mark 是否找到mark]
		"""
		aruco_dict = aruco.Dictionary_get(marktype)
		parameters = aruco.DetectorParameters_create()
		parameters.adaptiveThreshConstant = True

		gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
		corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
		centerX, centerY, cratio, markId, isFind = None, None, None, None, False
		if ids is not None and corners is not None:
			ids = ids.flatten()
			for i in range(len(ids)):
				if ids[i] in self.dic_specimen_id:
					aruco.drawDetectedMarkers(img, corners)
					centerX = int(corners[i][0][0][0] + corners[i][0][1][0] + corners[i][0][3][0] + corners[i][0][2][0]) / 4
					# cratio = int(corners[i][0][1][0] - corners[i][0][0][0])/120
					centerY = int(corners[i][0][0][1] + corners[i][0][1][1] + corners[i][0][3][1] + corners[i][0][2][1]) / 4
					markId, isFind = ids[i], True
		if TFT_FLAG is True:
			MARK_IMG=cv2.resize(img,(240, 240),interpolation=cv2.INTER_CUBIC) # 保存图片，设置TFT输出参数
			cv2.imwrite('/home/pi/Desktop/MicroScope/Temp/MARK_IMG.jpg',MARK_IMG)
			event.set()
		cv2.imshow('Find mark image:', img)
		return (centerX, centerY), markId, isFind


	def getImg(self, tcpSerSock, BUFSIZ):
		lena = tcpSerSock.recv(BUFSIZ)	# 先接收长度
		length = int(lena)	# 获取长度
		# print(lena)
		if not lena:	# 如果长度为空
			return
		tcpSerSock.send('ok'.encode())	# 发送OK，告诉服务器我接受到了
		hasLength = 0	# 现在接收了的长度
		bytes_stream = BytesIO()	# 开启一个内存IO流，把接收的写到内存里
		while hasLength < length:	# 若没接收完就继续接收
			lena = tcpSerSock.recv(BUFSIZ)  # 接收并获取长度
			hasLength += len(lena)
			if len(lena) < 0:	# 如果接收中断就中断
				break
			bytes_stream.write(lena)	# 将数据写入IO流
		# print('接收完了')
		roiimg = Image.open(bytes_stream)	# 读取这个完整的流变成图片
		img = cv2.cvtColor(np.asarray(roiimg), cv2.IMREAD_ANYCOLOR)	# 将PIL格式转换成CV的格式
		# roiimg.save('MicroScope_IMG.jpg')	# 接收完后保存成test.jpg
		return img


	def run(self, dic_scale_scope):
		"""[Loop 运行]	
		Arguments:
			dic_scale_scope {[dict]]} -- [key-value of keyboard 键盘操作键值对]
		"""
		global TFT_FLAG # 引入全局变量
		global EC11_FLAG0
		global EC11_FLAG1
		global Confirm  # Confirm mark position 确定mark中心位置为当前位置
		global isFind
		global lastCenter  # Center position of mark 记录找到mark中心位置
		global loadedImg # Wheather the image has been loaded 图片是否已经被加载
		global Timeout
		global Count_A
		global Count_B
		global First_key
		global comparekey
		global Speak

		# initialize the camera and grab a reference to the raw camera capture
		camera = PiCamera()
		camera.resolution = (320, 240)
		camera.framerate = 32
		# camera.saturation = 40
		camera.brightness = 50
		camera.sharpness  = 100
		camera.hflip = True
		camera.vflip = True
		rawCapture = PiRGBArray(camera, size=(320, 240))
		# allow the camera to warmup
		time.sleep(0.1)
		state = 0
		laststate = 0
		# 奇奇怪怪的状态量
		Second_key = False
		notFind = True
		CountC = True
		Speak = False

		# os.system('mplayer %s' % '1.1开机.mp3')

		# Confirm = False # 确定载玻片中心位置
		# lastCenter = None  # Center position of mark 记录找到mark中心位置
		# loadedImg = False # Wheather the image has been loaded 图片是否已经被加载
		lastcenter0, lastcenter1, lastID= 0, 0, None
		for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
			image = frame.array
			image = image[40:230, 50:260]
			# print(image.shape)
			rawCapture.truncate(0)
			center, tempIds, isFind = self.FindMark(image)
			if not isFind and notFind:
				if Speak:
					os.system('mplayer ./Sounds/2.载玻片.mp3')
				notFind = False

			keyBoard = cv2.waitKey(5)
			if keyBoard == dic_scale_scope['key'] or comparekey:
				state += 1
				print(state)
				comparekey = False
			
			if state != laststate:
				if state == 1:
					if Speak:
						os.system('mplayer ./Sounds/5.1请选取你感1.mp3')
					laststate = state
				if state == 2:
					if Speak:
						os.system('mplayer ./Sounds/5.2请选取你感2.mp3')
					laststate = state
				if state == 3:
					if Speak:
						os.system('mplayer ./Sounds/5.3对比.mp3')
					laststate = state
				else:
					laststate = state
			
			if First_key:
				if tempIds != None or keyBoard == dic_scale_scope['confirm'] or TFT_FLAG == False and not Confirm:
					HOST = '39.106.65.18'		# '169.254.126.150' # '10.103.29.194'  #服务器外网IP地址
					PORT = 12453
					BUFSIZ = 4096
					ADDR = (HOST,PORT)
					tcpSerSock = socket(AF_INET, SOCK_STREAM)
					tcpSerSock.connect(ADDR)	# 连接目标

					Confirm = True				# Confim. 确定当前位置为mark的起始位置
					lastCenter = center			# Record. 记录当前mark中心位置
					print('Confirm the mark')
					First_key = False
					loadedImg = True
					TFT_FLAG = False
			else:
				Second_key = True
	
			# 分手炮 以及断开socket连接
			if Second_key:
				if tempIds != lastID and tempIds != None or keyBoard == dic_scale_scope['quit'] or Timeout:
					print('Timeout')
					if Speak:
						os.system('mplayer ./Sounds/4.3更换载玻片.mp3')
					tcpSerSock.send('{}'.format('1').encode())
					# reply = tcpSerSock.recv(BUFSIZ)
					# if 'ok' == reply.decode(): 
					# 	self.result_img = self.getImg(tcpSerSock, BUFSIZ)
					TFT_FLAG = True
					Confirm = False
					self.reload()
					First_key = True
					Second_key = False
					Count_B, Count_A = 0, 0
					# tcpSerSock.close()
			
			# 获取图像
			if Confirm and isFind and loadedImg and center[0] != None:
				if abs(lastcenter0-center[0])>0.3 or abs(lastcenter1-center[1])>0.3 :
					tcpSerSock.send('{},{},{},{},{},{}'.format(tempIds, center[0], center[1], Count_B, Count_A, state).encode())
					lastcenter0, lastcenter1 = center[0], center[1]
				else:
					tcpSerSock.send('{},{},{},{},{},{}'.format(tempIds, lastcenter0, lastcenter1, Count_B, Count_A, state).encode())

				reply = tcpSerSock.recv(BUFSIZ)
				if 'ok' == reply.decode(): 
					self.result_img = self.getImg(tcpSerSock, BUFSIZ)
					cv2.imwrite("/home/pi/Desktop/MicroScope/Temp/MicroScope_IMG.jpg", self.result_img)
					event.set()
					cv2.imshow('MicroScope', self.result_img)
				if not notFind:
					if Speak:
						os.system('mplayer ./Sounds/3.图像已经出.mp3')
				# 语音状态量
				notFind = True
	
			if abs(1-4*(Count_A//3)+4*(Count_B//3))>12 and CountC:
				if Count_A > Count_B:
					if Speak:
						os.system('mplayer ./Sounds/4.2这也太模糊细.mp3')
				if Count_B > Count_A:
					if Speak:
						os.system('mplayer ./Sounds/4.1这也太模糊粗.mp3')
				CountC = False
			if abs(1-4*(Count_A//3)+4*(Count_B//3))<12:
				CountC = True
				

			if tempIds != None:
				lastID = tempIds
			
			if state == 4:
				state, laststate = 0, 0

	def reload(self):
		clear_img = np.zeros((self.microscope_w, self.microscope_h, 3), np.uint8) + 255
		cv2.imshow('MicroScope', clear_img)
	
	def release(self):
		self.cap.release()
		cv2.destroyAllWindows()
 
 
if __name__ == '__main__':
	EC11_FLAG0 = 0 # 全局变量 旋转编码器方向标志位 0：初始状态 1：正转 2：反转
	EC11_FLAG1 = 0 # 全局变量 旋转编码器方向标志位 0：初始状态 1：正转 2：反转
	Count_A = 0
	Count_B = 0
	TFT_FLAG = True # 显示屏选择显示方式 TRUE：摄像头 FALSE：显微镜图
	First_key = True
	Timeout = False
	isFind = False
	comparekey = False # 对比功能按键
	Confirm = False # 确定载玻片中心位置
	lastCenter = None  # Center position of mark 记录找到mark中心位置
	loadedImg = False # Wheather the image has been loaded 图片是否已经被加载
	Speak = False # 语音提示
	dic_scale_scope = {
		# The key-value scale of microscope 调节显微镜的键值对
		'confirm': ord('='), # Confirm mark 确认mark位置
		'quit': ord('q'), # quit the program 退出 程序
		'key': ord('a'),
		ord('w'): 1, # magnify 1 time 视野放大一倍
		ord('s'): -1,# shrink 1 time 视野缩小一倍
		ord('e'): 4, # magnify 4 time 视野放大四倍
		ord('d'): -4,# shrink 4 time 视野缩小四倍
		ord('r'): 10,# magnify 10 time 视野放大十倍
		ord('f'): -10,# shrink 10 time 视野缩小十倍
	}

	micro_w, micro_h = 240, 240 # The width and height of microscope 显微镜视口宽度和高度
	micro_posX, micro_posY = 500, 500 # The x postion of microscope 显微镜视口x、y坐标
	
	dic_specimen_id = { # The key-value : id - specimen photo 索引和标本对应
		1 : 'Onion.jpg', # Onion 洋葱切片
		2 : 'Paramecium.jpg', # Paramecium.jpg 草履虫标本
		3 : 'SpinalCord.jpg' # SpinalCord.jpg 脊髓切片
	}

	# TFT显示线程
	event = threading.Event() # 摄像头图片保存与TFT屏图片读取互斥标志位
	lock = threading.Lock() # 旋转编码器全局变量上锁，防止线程间同时操作变量
	tft = Function0_Thread()
	ec11 = Function1_Thread()
	stopstate = Stop_Thread()
	tft.start()
	ec11.start()
	stopstate.start()
	
	microScope =  MicroScope(micro_w, micro_h, micro_posX, micro_posY, dic_specimen_id)
	while True:
		microScope.run(dic_scale_scope)
	microScope.release()
	del microScope
